#include "complementary_filter.h"
#include "../utils/QueueUtil.h"

//Queue globalQueue;


void initialize(ComplementaryFilterV2 *filter) {
    filter->gain_acc = 0.0004;          // 加速度初始增益
    filter->prev_gain_acc = 0.0;        // 上一增益值
    filter->gain_mag = 0.0004;          // 磁力计初始增益
    filter->bias_alpha = 0.001;         // 零偏估计参数
    filter->do_bias_estimation = 1;     // 是否零偏估计
    filter->do_adaptive_gain = 1;       // 是否自适应计算增益
    filter->do_gravity_autoscale = 1;   // 是否自适应调整重力加速度
    filter->gravity = 9.81;             // 重力加速度
    filter->initialized = 0;            // 是否初始化
    filter->steady_state = 0;           // 是否处于稳定状态
    filter->partial_steady_state = 0;   // 是否处于半稳定状态
    filter->q->q0 = 1.0;                // 朝向四元数 q0是实部
    filter->q->q1 = 0.0;
    filter->q->q2 = 0.0;
    filter->q->q3 = 0.0;
    filter->a_filt->x = 0.0;             // 滤波后的加速度
    filter->a_filt->y = 0.0;
    filter->a_filt->z = 0.0;
    filter->a_prev->x = 0.0;             // 前一个加速度
    filter->a_prev->y = 0.0;
    filter->a_prev->z = 0.0;
    filter->w_prev->x = 0.0;             // 前一个角速度
    filter->w_prev->y = 0.0;
    filter->w_prev->z = 0.0;
    filter->w_bias->x = 0.0;             // 角速度零偏
    filter->w_bias->y = 0.0;
    filter->w_bias->z = 0.0;
    filter->time = 0.0;                 // 当前时间
    filter->time_steady = 0.0;          // 稳定状态时长
    filter->initial_settle_time = 2.0;  // 加速度增益的初始时间
    filter->angle_velocity = 0;         // 角速度
}

//// Function to initialize Vector3
//Vector3* initVector3(double x, double y, double z) {
//    Vector3 *vec = (Vector3*)malloc(sizeof(Vector3));
//    vec->x = x;
//    vec->y = y;
//    vec->z = z;
//    return vec;
//}
//
//// Function to initialize Quaternion
//Quaternion* initQuaternion(double q0, double q1, double q2, double q3) {
//    Quaternion *quat = (Quaternion*)malloc(sizeof(Quaternion));
//    quat->q0 = q0;
//    quat->q1 = q1;
//    quat->q2 = q2;
//    quat->q3 = q3;
//    return quat;
//}

// Function to initialize ComplementaryFilterV2
void initComplementaryFilter(ComplementaryFilterV2 * filter) {
    filter->gain_acc = 0.0004;          // 加速度初始增益
    filter->prev_gain_acc = 0.0;        // 上一增益值
    filter->gain_mag = 0.0004;          // 磁力计初始增益
    filter->bias_alpha = 0.001;         // 零偏估计参数
    filter->do_bias_estimation = 1;     // 是否零偏估计
    filter->do_adaptive_gain = 1;       // 是否自适应计算增益
    filter->do_gravity_autoscale = 1;   // 是否自适应调整重力加速度
    filter->gravity = 9.81;             // 重力加速度
    filter->initialized = 0;            // 是否初始化
    filter->steady_state = 0;           // 是否处于稳定状态
    filter->partial_steady_state = 0;   // 是否处于半稳定状态
    filter->q = initQuaternion(1.0, 0.0, 0.0, 0.0); // Default quaternion values
    filter->a_filt = initVector3(0.0, 0.0, 0.0); // Default Vector3 values
    filter->a_prev = initVector3(0.0, 0.0, 0.0); // Default Vector3 values
    filter->w_prev = initVector3(0.0, 0.0, 0.0); // Default Vector3 values
    filter->w_bias = initVector3(0.0, 0.0, 0.0); // Default Vector3 values
    filter->time = 0.0;
    filter->time_steady = 0.0;
    filter->initial_settle_time = 2.0;
    filter->angle_velocity = 0.0;
}

int set_gain_acc(ComplementaryFilterV2 *filter, double gain) {
    if (0.0 <= gain && gain <= 1.0) {
        filter->gain_acc = gain;
        return 1;
    } else {
        return 0;
    }
}

int set_gain_mag(ComplementaryFilterV2 *filter, double gain) {
    if (0.0 <= gain && gain <= 1.0) {
        filter->gain_mag = gain;
        return 1;
    } else {
        return 0;
    }
}

int set_bias_alpha(ComplementaryFilterV2 *filter, double bias_alpha) {
    if (0.0 <= bias_alpha && bias_alpha <= 1.0) {
        filter->bias_alpha = bias_alpha;
        return 1;
    } else {
        return 0;
    }
}

void set_initial_settle_time(ComplementaryFilterV2 *filter, double settle_time) {
    filter->initial_settle_time = settle_time;
}

int get_steady_state(const ComplementaryFilterV2 *filter) {
    return filter->steady_state;
}

Quaternion invert_quaternion(Quaternion* q) {
    Quaternion r = {q->q0, -q->q1, -q->q2, -q->q3};
    return r;
}

void set_orientation(const ComplementaryFilterV2 *filter, double q0, double q1, double q2, double q3) {
    filter->q->q0 = q0;
    filter->q->q1 = -q1;
    filter->q->q2 = -q2;
    filter->q->q3 = -q3;
}

Quaternion get_orientation(const ComplementaryFilterV2 *filter){
    Quaternion result = {filter->q->q0, -filter->q->q1, -filter->q->q2, -filter->q->q3};
    return result;
}

void normalize_vector(Vector3 *v) {
    double l = sqrt(v->x * v->x + v->y * v->y + v->z * v->z);
    if (l != 0.0) {
        v->x /= l;
        v->y /= l;
        v->z /= l;
    }
}
Vector3 rotate_vector_by_quaternion(Vector3 *acc, Quaternion *q) {
    //inverse
    Quaternion r = invert_quaternion(q);

    Vector3 g;
    g.x = (r.q0 * r.q0 + r.q1 * r.q1 - r.q2 * r.q2 - r.q3 * r.q3) * acc->x
            + 2.0 * (r.q1 * r.q2) * acc->y - 2.0 * (r.q0 * r.q2) * acc->z;
    g.y = 2.0 * (r.q1 * r.q2) * acc->x
            + (r.q0 * r.q0 - r.q1 * r.q1 + r.q2 * r.q2 -r.q3 * r.q3) * acc->y
            + 2.0 * (r.q0 * r.q1) * acc->z;
    g.z = 2.0 * (r.q0 * r.q2) * acc->x
            + 2.0 * (r.q0 * r.q1) * acc->y
            + (r.q0 * r.q0 - r.q1 * r.q1 - r.q2 * r.q2 + r.q3 * r.q3) * acc->z;
    return g;
}

void scale_quaternion(double gain, Quaternion *q) {
    if (q->q0 < 0.0) { // 0.9
        // Slerp (Spherical linear interpolation):
        double angle = acos(q->q0);
        double a = (sin(angle * (1.0 - gain)) / sin(angle));
        double b = (sin(angle * gain) / sin(angle));
        q->q0 = a + b * (q->q0);
        q->q1 *= b;
        q->q2 *= b;
        q->q3 *= b;
    } else {
        // Lerp (Linear interpolation):
        q->q0 = (1.0 - gain) + gain * (q->q0);
        q->q1 *= gain;
        q->q2 *= gain;
        q->q3 *= gain;
    }

    normalize_quaternion(q);
}

void quaternion_multiplication(Quaternion *q, Quaternion *p, Quaternion *d) {
    q->q0 = p->q0 * d->q0 - p->q1 * d->q1 - p->q2 * d->q2 - p->q3 * d->q3;
    q->q1 = p->q0 * d->q1 + p->q1 * d->q0 + p->q2 * d->q3 - p->q3 * d->q2;
    q->q2 = p->q0 * d->q2 - p->q1 * d->q3 + p->q2 * d->q0 + p->q3 * d->q1;
    q->q3 = p->q0 * d->q3 + p->q1 * d->q2 - p->q2 * d->q1 + p->q3 * d->q0;
}

void filter_acc(Vector3 *a_filt, double ax, double ay, double az, double dt) {
    // simple IIR filter to acceleration
    double iir_alpha = 1.0 - exp(-dt / ACC_FILT_TIMECONSTANT);
    a_filt->x = iir_alpha * ax + (1.0 - iir_alpha) * a_filt->x;
    a_filt->y = iir_alpha * ay + (1.0 - iir_alpha) * a_filt->y;
    a_filt->z = iir_alpha * az + (1.0 - iir_alpha) * a_filt->z;
}

void update_biases(Vector3 *w_bias, double bias_alpha, double time_steady, double wx, double wy, double wz) {
    if (time_steady > STEADY_WAIT_THRESHOLD) {
        w_bias->x += bias_alpha * (wx - w_bias->x);
        w_bias->y += bias_alpha * (wy - w_bias->y);
        w_bias->z += bias_alpha * (wz - w_bias->z);
    }
}

void autoscale_gravity(int partial_steady_state, Vector3 *a_filt, double *gravity) {
    if (partial_steady_state) {
        double acc_length = sqrt(a_filt->x * a_filt->x + a_filt->y * a_filt->y + a_filt->z * a_filt->z);
        if (fabs(acc_length - GRAVITY) < GRAV_AUTOSCALE_THRESHOLD) {
            *gravity = *gravity * (1.0 - GRAV_AUTOSCALE_ALPHA) + GRAV_AUTOSCALE_ALPHA * acc_length;
        }
    }
}

/**
 * 检查当前IMU数据是否处于稳定状态
 * 稳定状态判定标准是    acc合力与filter->g差值<0.1
 *                  且  各轴分量与滤波后的acc变化量均<0.05
 *                  且   各轴分量与上一帧的acc变化量<0.01
 *                  且   陀螺仪角速度示数与上一帧 <0.01
 *                  且   陀螺仪角速度示数与bias差值 <0.01
 * @param ax
 * @param ay
 * @param az
 * @param wx
 * @param wy
 * @param wz
 * @param w_prev
 * @param a_prev
 * @return 1:稳定 0:非稳定
 */
int check_state(ComplementaryFilterV2 *filter, double ax, double ay, double az, double wx, double wy, double wz) {
    double acc_length = sqrt(ax * ax + ay * ay + az * az);

    int acc_th = fabs(acc_length - filter->gravity) < ACCELERATION_THRESHOLD;
    int acc_component_steady = fabs(ax - filter->a_filt->x) < DELTA_ACCELERATION_THRESHOLD ||
                               fabs(ay - filter->a_filt->y) < DELTA_ACCELERATION_THRESHOLD ||
                               fabs(az - filter->a_filt->z) < DELTA_ACCELERATION_THRESHOLD;
    int acc_delta_th = fabs(ax - filter->a_prev->x) < DELTA_ACCELERATION_THRESHOLD ||
                       fabs(ay - filter->a_prev->y) < DELTA_ACCELERATION_THRESHOLD ||
                       fabs(az - filter->a_prev->z) < DELTA_ACCELERATION_THRESHOLD;
    int gyro_delta_th = fabs(wx - filter->w_prev->x) < DELTA_ANGULAR_VELOCITY_THRESHOLD ||
                        fabs(wy - filter->w_prev->y) < DELTA_ANGULAR_VELOCITY_THRESHOLD ||
                        fabs(wz - filter->w_prev->z) < DELTA_ANGULAR_VELOCITY_THRESHOLD;
    int gyro_th = fabs(wx - filter->w_bias->x) < ANGULAR_VELOCITY_THRESHOLD ||
                  fabs(wy - filter->w_bias->y) < ANGULAR_VELOCITY_THRESHOLD ||
                  fabs(wz - filter->w_bias->z) < ANGULAR_VELOCITY_THRESHOLD;

    set_vector3(wx, wy, wz, filter->w_prev);
    set_vector3(ax, ay, az, filter->a_prev);

    return acc_th && acc_component_steady && acc_delta_th && gyro_delta_th && gyro_th;
}

double get_adaptive_gain(double alpha, double ax, double ay, double az, double dt,
                         double *w_prev, int do_adaptive_gain, double time, double initial_settle_time) {
    if (do_adaptive_gain) {
        double a_mag = sqrt(ax * ax + ay * ay + az * az);
        double w_mag = sqrt(w_prev[0] * w_prev[0] + w_prev[1] * w_prev[1] + w_prev[2] * w_prev[2]);
        double error = fabs(a_mag - GRAVITY) / GRAVITY;

        // Remaining part of get_adaptive_gain function goes here

    } else {
        return alpha;
    }
}

/**
 * 根据加速度数据来初始化得到方向四元数
 * @param ax
 * @param ay
 * @param az
 * @return
 */
void get_measurement(double ax, double ay, double az, Quaternion * q) {
    Vector3 accel = {ax, ay, az};
    normalize_vector(&accel);

    if (az >= 0.0) {
        double q0_meas = sqrt((az + 1.0) * 0.5);
        q->q0 = 0.0;
        q->q1 = q0_meas;
        q->q2 = -ay / (2.0 * q0_meas);
        q->q3 = ax / (2.0 * q0_meas);
    } else {
        double x = sqrt((1.0 - az) * 0.5);
        q->q0 = ax / (2.0 * x);
        q->q1 = -ay / (2.0 * x);
        q->q2 = x;
        q->q3 = 0.0;
    }
}

void set_vector3(double ax, double ay, double az, Vector3 * vector3) {
    vector3->x = ax;
    vector3->y = ay;
    vector3->z = az;
}

void normalize_quaternion(Quaternion *q) {
    double norm = sqrt(q->q0 * q->q0 + q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3);
    q->q0 /= norm;
    q->q1 /= norm;
    q->q2 /= norm;
    q->q3 /= norm;
}

Quaternion get_prediction(Quaternion *q, Vector3 *w_bias, double wx, double wy, double wz, double dt) {
    double wx_unb = wx - w_bias->x;
    double wy_unb = wy - w_bias->y;
    double wz_unb = wz - w_bias->z;

    Quaternion q_pred;
    q_pred.q0 = q->q0 + 0.5 * dt * (wx_unb * q->q1 + wy_unb * q->q2 + wz_unb * q->q3);
    q_pred.q1 = q->q1 + 0.5 * dt * (-wx_unb * q->q0 - wy_unb * q->q3 + wz_unb * q->q2);
    q_pred.q2 = q->q2 + 0.5 * dt * (wx_unb * q->q3 - wy_unb * q->q0 - wz_unb * q->q1);
    q_pred.q3 = q->q3 + 0.5 * dt * (-wx_unb * q->q2 + wy_unb * q->q1 - wz_unb * q->q0);

    normalize_quaternion(&q_pred);

    return q_pred;
}

void get_acc_correction(Quaternion *dq, Quaternion *p, double ax, double ay, double az) {
    Vector3 accel;
    accel.x = ax;
    accel.y = ay;
    accel.z = az;
    normalize_vector(&accel);

    Vector3 g = rotate_vector_by_quaternion(&accel, p);
    double dq0 = sqrt((g.z + 1.0) * 0.5);

    dq->q0 = dq0;
    dq->q1 = -g.y / (2.0 * dq0);
    dq->q2 = g.x / (2.0 * dq0);
    dq->q3 = 0.0;

}

double max(double a, double b) {
    return a > b ? a : b;
}

double get_adaptive_gain(ComplementaryFilterV2 *filter, double ax, double ay, double az, double dt) {
    double alpha = filter->gain_acc;

    if (filter->do_adaptive_gain) {
        double a_length = sqrt(ax * ax + ay * ay + az * az);
        double w_length = sqrt(filter->w_prev->x * filter->w_prev->x
                                + filter->w_prev->y * filter->w_prev->y
                                + filter->w_prev->z * filter->w_prev->z);
        double error = fabs((a_length - filter->gravity) / filter->gravity);

        double gain_iir_alpha = 1.0 - exp(-dt / 0.15); // 0.15s time constant filtering for gain

        // scaling of 0.13 at error of 5% = 0.5 m/s^2
        // initial settle gain factor is constant followed by slope down to 1
        double new_gain = 0.0;
        if (filter->time_steady > STEADY_WAIT_THRESHOLD) {
            new_gain = 8.0 * alpha;
        } else {
            double coe;
            if(filter->time < filter->initial_settle_time) {
                coe =  max(15.0 - filter->time / filter->initial_settle_time * 14.0, 8.0);
            } else {
                coe = 1;
            }
            new_gain = exp(-40.0 * error - 1.0 * w_length) * alpha * coe;
        }

        // 1st order filter of gain when increasing
        double gain = (filter->prev_gain_acc > new_gain) ? new_gain : (gain_iir_alpha * new_gain + (1.0 - gain_iir_alpha) * filter->prev_gain_acc);
        filter->prev_gain_acc = gain;
        return gain;
    } else {
        return alpha;
    }
}

void update(ComplementaryFilterV2 *filter, double ax, double ay, double az, double wx, double wy, double wz, double dt) {
    if (!filter->initialized) {
        // First time - ignore prediction
        Quaternion measurement;
        get_measurement(ax, ay, az, filter->q);
        set_vector3(ax, ay, az, filter->a_filt);
        set_vector3(ax, ay, az, filter->a_prev);
        filter->initialized = 1;
        return;
    }
    //在这里检查下filter中的各个值

    // 滤波调整后的三轴值
    double axf, ayf, azf;
    filter_acc(filter->a_filt, ax, ay, az, dt);
    axf = filter->a_filt->x;
    ayf = filter->a_filt->y;
    azf = filter->a_filt->z;
    int steady_state = check_state(filter, ax, ay, az, wx, wy, wz);
    filter->time_steady = steady_state ? (filter->time_steady + dt) : 0.0;

    if (filter->do_bias_estimation) {
        update_biases(filter->w_bias, filter->bias_alpha, filter->time_steady, wx, wy, wz);
    }

    Quaternion pred;
    if (filter->do_gravity_autoscale) {
        autoscale_gravity(steady_state, filter->a_filt, &filter->gravity);
    }


    pred = get_prediction(filter->q, filter->w_bias, wx, wy, wz, dt);

    Quaternion dq_acc;
    get_acc_correction(&dq_acc, &pred, axf, ayf, azf);

    double gain = get_adaptive_gain(filter, axf, ayf, azf, dt);

    scale_quaternion(gain, &dq_acc);

    quaternion_multiplication(filter->q, &pred, &dq_acc);

    normalize_quaternion(filter->q);

    //todo: 再顺便计算一下角速度，平滑时，速度滤波要用

    //todo: 四元数和角速度分别存入两个全局队列。两个队列长度均为 100 = 200Hz * 15f/60fps
//    insertQ(&globalQueue, filter->q);

    // todo:另外起一个线程，做速度滤波，消费队列中数据

    filter->time += dt;
}



